#include "../Task/imu.h"

#include "../Device/imu.h"
#include "ahrs.h"
#include "sys.h"

short imu_accel[3], imu_gyro[3];
float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z;

/**
 * @brief 读取MPU6500数据并进行姿态解算
 * @param argument: 未使用
 * @retval 无
 */
void MPU6500(void *argument) {
  const uint32_t delay_tick = osKernelGetTickFreq() / IMU_SCAN_FREQ;

  MPU6500_Init();

  uint32_t tick = osKernelGetTickCount();
  while (1) {
    tick += delay_tick;

    MPU6500_GetData(imu_accel, imu_gyro);

    AHRS_Prase_IMU();
    AHRS_AnalyAngle();

    osDelayUntil(tick);
  }
}
